#include <stdio.h>
#include <string>
#include <signal.h>
#include <unistd.h>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>

#include "CBallCircle.h"
#include "CTargetInfo.h"
#include "CVideoFrame.h"
#include "CVideoFrameQueue.h"
#include "CConnectionServer.h"
#include "CGpioLed.h"
#include "CBallDetector.h"
#include "CFrameGrinder.h"

CBallDetector::CBallDetector()
{
}

CBallDetector::CBallDetector(const CBallDetector& orig)
{
}

CBallDetector::~CBallDetector()
{
}

void CBallDetector::detectBlobs(CVideoFrame * pFrame)
{
    try
    {
        unsigned int i = 0;
        cv::Mat img_hsv, red_blob, blue_blob, green_blob, dstA, dstB;
        struct timespec time1, time2, time3, time4, time5, time6, time7, time8, time9, time10;
        CFrameGrinder::getTicks(&time1);

        // Work on a clone so the orignal image is not altered until the end
        cv::Mat tempImage = pFrame->m_frame.clone();

        CFrameGrinder::getTicks(&time2);
        // Reduce the noise so we find the larger shapes
        // EXPERIMENT -- this line is very costly - can we see the ball without it?
        /// ???????? GaussianBlur(tempImage, tempImage, cv::Size(9, 9), 2, 2);

        CFrameGrinder::getTicks(&time3);
        // RBG is flawed as a way to filter based on color because the brightness is combined 
        // with the color info. 
        // Not so with HSV, where Hue and Saturation are maintained separately
        // OpenCV has a handy conversion from RGB to HSV
        cv::cvtColor(tempImage, img_hsv, CV_BGR2HSV);

        CFrameGrinder::getTicks(&time4);
        // Filter out all but Blue hue
        cv::inRange(img_hsv, cv::Scalar(110, 90, 90), cv::Scalar(120, 255, 255), blue_blob);

        CFrameGrinder::getTicks(&time5);
        // Filter out all but red
        cv::inRange(img_hsv, cv::Scalar(0, 75, 75), cv::Scalar(5, 255, 255), dstA);
        cv::inRange(img_hsv, cv::Scalar(175, 75, 75), cv::Scalar(180, 255, 255), dstB);
        cv::bitwise_or(dstA, dstB, red_blob);

        // Filter out all but Green hue
        cv::inRange(img_hsv, cv::Scalar(78, 95, 90), cv::Scalar(82, 255, 255), green_blob);

        //cv::imshow("green_blob", green_blob);
        //cv::waitKey(0);

        CFrameGrinder::getTicks(&time6);
        //Find the contours. Use the contourOutput Mat so the original image doesn't get overwritten
        std::vector<std::vector<cv::Point> > blueContours, redContours, greenContours;
        cv::findContours(blue_blob, blueContours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
        cv::findContours(red_blob, redContours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
        cv::findContours(green_blob, greenContours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);

        CBallCircle tempBallCircleBlue;
        CBallCircle tempBallCircleRed;
        bool isBlueFound = false;
        bool isRedFound = false;
        CBallCircle bestBallCircleBlue;
        CBallCircle bestBallCircleRed;
        std::vector<CBallCircle> listPossibleBallCircle;
        std::vector<cv::Point> contours_poly;
        for (i = 0; i < blueContours.size(); i++)
        {
            approxPolyDP(cv::Mat(blueContours[i]), contours_poly, 3, true);
            minEnclosingCircle((cv::Mat) contours_poly, tempBallCircleBlue.m_ptCenter, tempBallCircleBlue.m_radius);
            if (!tempBallCircleBlue.isSmallerThanBallAtCenterLocationInView())
            {
                listPossibleBallCircle.push_back(tempBallCircleBlue);
            }
        }
        if (listPossibleBallCircle.size() == 1)
        {
            // Perfect - Found exactly one candidate 
            isBlueFound = true;
            bestBallCircleBlue = listPossibleBallCircle.at(0);
        }
        else if (listPossibleBallCircle.size() > 1)
        {
            // Take first candidate that is about the right size
            for (i = 0; i < listPossibleBallCircle.size(); i++)
            {
                if (!listPossibleBallCircle.at(i).isLargerThanBallAtCenterLocationInView())
                {
                    isBlueFound = true;
                    bestBallCircleBlue = listPossibleBallCircle.at(i);
                }
            }
        }
        listPossibleBallCircle.clear();
        for (i = 0; i < redContours.size(); i++)
        {
            approxPolyDP(cv::Mat(redContours[i]), contours_poly, 3, true);
            minEnclosingCircle((cv::Mat) contours_poly, tempBallCircleRed.m_ptCenter, tempBallCircleRed.m_radius);
            if (!tempBallCircleRed.isSmallerThanBallAtCenterLocationInView())
            {
                listPossibleBallCircle.push_back(tempBallCircleRed);
            }
        }
        if (listPossibleBallCircle.size() == 1)
        {
            // Perfect - Found exactly one candidate 
            isRedFound = true;
            bestBallCircleRed = listPossibleBallCircle.at(0);
        }
        else if (listPossibleBallCircle.size() > 1)
        {
            // Take first candidate that is about the right size
            for (i = 0; i < listPossibleBallCircle.size(); i++)
            {
                if (!listPossibleBallCircle.at(i).isLargerThanBallAtCenterLocationInView())
                {
                    isRedFound = true;
                    bestBallCircleRed = listPossibleBallCircle.at(i);
                }
            }
        }

        CFrameGrinder::getTicks(&time7);

        int iIndexOfLargestRect = -1;
        float maxArea = 0.0;
        float fTemp;
        std::vector<cv::RotatedRect> minRect(greenContours.size());
        for (int i = 0; i < greenContours.size(); i++)
        {
            minRect[i] = cv::minAreaRect(cv::Mat(greenContours[i]));
            fTemp = minRect[i].size.width * minRect[i].size.height;
            if (fTemp > maxArea)
            {
                iIndexOfLargestRect = i;
                maxArea = fTemp;
            }
        }
        pFrame->m_targetInfo.init();
        if (iIndexOfLargestRect != -1)
        {
            pFrame->m_targetInfo.updateTargetInfo(
                    isBlueFound,
                    bestBallCircleBlue,
                    isRedFound,
                    bestBallCircleRed,
                    minRect[iIndexOfLargestRect]);

            pFrame->updateAnnotationInfo(
                    bestBallCircleBlue,
                    bestBallCircleRed,
                    minRect[iIndexOfLargestRect]);
        }
        m_gpioLed.setVisionOkLED(pFrame->m_targetInfo.isVisionOriented(), pFrame->m_timeRemovedFromQueue[(int) CVideoFrame::FRAME_QUEUE_WAIT_FOR_BLOB_DETECT]);

        /*printf("%02fs   %02fs   %02fs   %02fs   %02fs   %02fs   %02fs   %02fs   \n",
                getDeltaTimeSeconds(time1, time2),
                getDeltaTimeSeconds(time2, time3),
                getDeltaTimeSeconds(time3, time4),
                getDeltaTimeSeconds(time4, time5),
                getDeltaTimeSeconds(time5, time6),
                getDeltaTimeSeconds(time6, time7),
                getDeltaTimeSeconds(time7, time8),
                getDeltaTimeSeconds(time8, time9));
         * */
    }
    catch (...)
    {
    }
}
